/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CScanMeasurementSet.h"
#define pdata "../../../staticdata/datasets/Avant635/08_12_2010/Avant635.MaCI_Position.Odometry_08_12_2010_11:25:13.txt"
#define rdata "../../../staticdata/datasets/Avant635/08_12_2010/Avant635.MaCI_Ranging.LaserSICK_08_12_2010_11:25:13.txt"

int main(){
	// Init Debug lib
	debugInit();
	debugSetGlobalDebugLvl(6);
	
	//CScanMeasurementSet set("../../data/j2b2_measurement.txt");
	
	//fprintf(stderr,"SET1: There was %d measeurements, %d poses\n",set.ranges.size(), set.poses.size());
	
	CScanMeasurementSet set(pdata,rdata);
	//fprintf(stderr,"SET2:There was %d measeurements, %d poses\n",set2.ranges.size(), set2.poses.size());
	Grid::TScan s;
	Grid::pose p;
	
	for(int i=0;i<set.size();i++){
		if(set.getMeasurement(i, p, s)){
			dPrint(1,"%d Got measurement of size %d",i,s.N);
		}else{
			dPrint(1,"No measurement");
		}
	}
	
	
	return 0;
}